#!/usr/bin/python3

# Software License Agreement (BSD License)
# Copyright © 2019-2023 belongs to Shadow Robot Company Ltd.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without modification,
# are permitted provided that the following conditions are met:
#   1. Redistributions of source code must retain the above copyright notice,
#      this list of conditions and the following disclaimer.
#   2. Redistributions in binary form must reproduce the above copyright notice,
#      this list of conditions and the following disclaimer in the documentation
#      and/or other materials provided with the distribution.
#   3. Neither the name of Shadow Robot Company Ltd nor the names of its contributors
#      may be used to endorse or promote products derived from this software without
#      specific prior written permission.
#
# This software is provided by Shadow Robot Company Ltd "as is" and any express
# or implied warranties, including, but not limited to, the implied warranties of
# merchantability and fitness for a particular purpose are disclaimed. In no event
# shall the copyright holder be liable for any direct, indirect, incidental, special,
# exemplary, or consequential damages (including, but not limited to, procurement of
# substitute goods or services; loss of use, data, or profits; or business interruption)
# however caused and on any theory of liability, whether in contract, strict liability,
# or tort (including negligence or otherwise) arising in any way out of the use of this
# software, even if advised of the possibility of such damage.


from subprocess import check_output
import rospy
from sr_ur_arm_calibration_loader.sr_ur_arm_calibration_loader import SrUrLoadCalibration


class SrRobotDescriptionExceptions(Exception):
    pass


class PrefixNotCorrect(SrRobotDescriptionExceptions):
    pass


class SrConstructRobotDescription:
    TIMEOUT_IN_SECS = 60.0

    def __init__(self):
        self._load_robot_description_command = rospy.get_param("~load_robot_description_command")
        self._side = ''
        self._robot_description_params = {}
        self._sr_ur_load_calibration = ''
        arm_type = rospy.get_param("~arm_type")

        right_arm_ip = '10.8.1.1'
        left_arm_ip = '10.8.2.1'

        if self._arm_ip_params_loaded_before_timeout(self.TIMEOUT_IN_SECS):
            right_arm_ip, left_arm_ip = self._get_arm_ip_params(right_arm_ip, left_arm_ip)
        else:
            rospy.logwarn(f"No arm ip param was found on param server, using the following default arm ips:\n \
                           right arm ip: {right_arm_ip} - left arm ip: {left_arm_ip}")

        self._right_arm_params = {'prefix': 'ra', 'ip_address': right_arm_ip, 'arm_type': arm_type}
        self._left_arm_params = {'prefix': 'la', 'ip_address': left_arm_ip, 'arm_type': arm_type}

        if 'bimanual' in self._load_robot_description_command:
            self._side = 'both'
        else:
            self._side = rospy.get_param("~side")

        self._process_parameters()
        urdf = self._parse_xacro()

        rospy.set_param('robot_description', urdf.decode("utf-8"))
        rospy.set_param('robot_description_ready', "true")

    @staticmethod
    def _arm_ip_params_loaded_before_timeout(timeout_in_secs):
        start_time = rospy.get_time()
        while not rospy.has_param("/la_sr_ur_robot_hw/robot_ip") and not rospy.has_param("/ra_sr_ur_robot_hw/robot_ip"):
            rospy.sleep(0.1)
            if rospy.get_time() - start_time > timeout_in_secs:
                return False
        return True

    @staticmethod
    def _get_arm_ip_params(right_arm_ip, left_arm_ip):
        if rospy.has_param("/ra_sr_ur_robot_hw/robot_ip"):
            right_arm_ip = rospy.get_param("/ra_sr_ur_robot_hw/robot_ip")
        if rospy.has_param("/la_sr_ur_robot_hw/robot_ip"):
            left_arm_ip = rospy.get_param("/la_sr_ur_robot_hw/robot_ip")
        return right_arm_ip, left_arm_ip

    def _process_parameters(self):
        kinematics_configs = self._get_kinematics_config()
        if not self._side == 'both':
            self._robot_description_params['kinematics_config'] = kinematics_configs[0]
        else:
            self._robot_description_params['kinematics_config_right'] = kinematics_configs[0]
            self._robot_description_params['kinematics_config_left'] = kinematics_configs[1]

    def _get_kinematics_config(self):
        kinematics_configs = []
        if self._side == 'both':
            self._sr_ur_load_calibration = SrUrLoadCalibration([self._right_arm_params, self._left_arm_params])
        elif self._side == 'right':
            self._sr_ur_load_calibration = SrUrLoadCalibration([self._right_arm_params])
        elif self._side == 'left':
            self._sr_ur_load_calibration = SrUrLoadCalibration([self._left_arm_params])
        arms_info = self._sr_ur_load_calibration.get_calibration_files()
        if self._side != 'both':
            kinematics_configs.append(arms_info[0]['kinematics_config'])
        else:
            for arm in arms_info:
                kinematics_configs.append(arm['kinematics_config'])
        return kinematics_configs

    def _parse_xacro(self):
        xacro_command = self._load_robot_description_command
        for key, value in self._robot_description_params.items():
            xacro_command += (' ' + key + ':=' + str(value))
        return check_output([command.strip('\'') for command in xacro_command.split()])


if __name__ == "__main__":
    rospy.init_node("sr_construct_robot_description")
    sr_construct_robot_description = SrConstructRobotDescription()
